﻿#pragma once

#include <QMutex.h>
#include "RobotHandCommunication.h"
#include "TcpSocket.h"

class RobotHandKuka : public RobotHandCommunication
{
	Q_OBJECT
public:
	RobotHandKuka(QObject* parent);
	~RobotHandKuka();

	/****************************************************************************
	 * 功能: 初始化函数
	 * 传参: 初始化参数
	 * 返回值: 初始化并连接成功返回0, 参数错误返回 -1， 其它返回-2
	 ****************************************************************************/
	virtual int Init(const RobotParam& param) override;

	/****************************************************************************
	 * 功能: 重连接口
	 * 传参:
	 * 返回值: 重连是否成功返回0， 状态端口失败返回-1，命令端口失败返回-2, 全部失败返回-3
	 ****************************************************************************/
	virtual int Reconnecte() override;
	virtual int GetConnectedState() override;
	/****************************************************************************
	 * 功能: 停止接口，机械手停止运动并清理运动轨迹
	 * 传参:
	 * 返回值:
	 ****************************************************************************/
	virtual bool Stop() override;

	/****************************************************************************
	 * 功能: 暂停/恢复接口，机械手暂停，不清理轨迹，机械手恢复后继续运动
	 * 传参:
	 * 返回值:
	 ****************************************************************************/
	virtual bool Pause() override;
	virtual bool Resume() override;

	/****************************************************************************
	 * 功能: 设置和获取当前的TCPBase(未启用)
	 * 传参:
	 * 返回值:
	 ****************************************************************************/
	virtual bool SetTcpBase(const QString& info) override;
	virtual QString GetTcpBase() override;

	/****************************************************************************
	 * 功能: 设置DO
	 * 传参: index: DO编号(1-n), value: true=1,false=0;
	 * 返回值:
	 ****************************************************************************/
	virtual bool SetDo(unsigned int index, bool value) override;

	/****************************************************************************
	 * 功能: 夹具动作/预定义动作
	 * 传参: index: 预先定义的动作编号（由机械手内部程序定义）
	 * 返回值:
	 ****************************************************************************/
	virtual bool SetGrip(unsigned int index) override;

	/****************************************************************************
	 * 功能: 移动指令
	 * 传参:
	 * 返回值:
	 ****************************************************************************/
	virtual bool MoveJL(const QList<MoveJLInfo>& moves) override;

	/****************************************************************************
	 * 功能: 特殊指令
	 * 传参:
	 * 返回值:
	 ****************************************************************************/
	virtual bool ExecCmd(const QString& cmd, const QString& para, QString& result) override;

private slots:
	// 状态端口消息读取
	void OnStatusDataReady(const QByteArray& data);
	void OnStatusConnectionLost();

	// 状态端口消息读取
	void OnCommandDataReady(const QByteArray& data);
	void OnCommandConnectionLost();

private:
	bool SendCmdData(int type, const QByteArray& data);

	bool WaitForCmdAns(int index, int type);
	/****************************************************************************
	 * 功能: 校验命令返回值是否正确
	 * 传参: 命令编号：index, 命令类型: type, 返回消息：ans
	 * 返回值: 0: 消息校验正常， 1：还需要继续等待回复，-1：消息校验失败，无法收到回复
	 ****************************************************************************/
	int CheckCmdReturnVal(int index, int type, const QByteArray& ans);

	void ParaseRobotStateData();

	QString GetFaultMsg(int fault);
private:	// 状态解析函数
	QByteArray ParseJoints(const QByteArray& d);
	QByteArray ParseTargets(const QByteArray& d);
	int ParsePointId(const QByteArray& d);
	int ParseMoving(const QByteArray& d);
	int ParseToolId(const QByteArray& d);
	int ParseFaultCode(const QByteArray& d);
	QString ParseGi16(const QByteArray& d);
	QString ParseGo16(const QByteArray& d);
	QString ParseGi16Extend(const QByteArray& d);
	QString ParseGo16Extend(const QByteArray& d);

private:
	unsigned int m_msgIndex = 0;
	TcpSocket* m_state = nullptr;
	TcpSocket* m_cmds = nullptr;
	//RobotHandStatusParser* statusParser = nullptr;
	QMutex* sendMutex = nullptr;
	QMutex cmdDataMutex;
	QByteArrayList cmdRecvData;

	QByteArray m_lastStateData;
	int m_stateTimes = 0;

	QString Di;
	QString Do;
};
